//#include "svm.h"
#include "FeatureResponse.h"
//#include "./mysvm.cpp"
//#include "./mysvm.h"
#include "./orgsvm.cpp"
#include "./orgsvm.h"
#include "./Classifier.h"
#include <math.h>
#include <stdio.h>
#include <sys/time.h>
#include <iostream>
#include <time.h>
#include <fstream>
#include <stdlib.h>
#include "./SVMClassifier.h"
//OpenCV
//#include "cv.h"
//#include "highgui.h"



#define Malloc(type,n) (type *)malloc((n)*sizeof(type))

namespace findobject{

SVMClassifier::SVMClassifier() {

}

SVMClassifier::~SVMClassifier() {
	free(x_space);
	delete[] prob.y;
	delete[] prob.x;
	svm_free_and_destroy_model(&model);
	//svm_destroy_param(&param);
}



FeatureResponse * SVMClassifier::evaluate(FeatureResponse * data){

	svm_node * x = new svm_node[data->length + 1];
	for(double i = 0; i < data->length; i++)
	{
		x[(int)i].index = i;
		x[(int)i].value = data->response[(int)i];
	}
	x[data->length].index=-1;
	x[data->length].value = 0;
	double vl = scaling*svm_predict(model,x);
	delete[] x;
	float * t = new float[1];
	if(isnan(vl)){
		t[0] = 0;
	}else{
		t[0] = (float)vl;
	}
	return new FeatureResponse(t,1);
	//return NULL;
};

void SVMClassifier::train(FeatureResponse ** target, FeatureResponse ** data,float * data_weights, int nr_data){

float powerfuncval = 0.83f;

	if(nr_data<2)
	{
		printf("ERROR:nr_data < 2 is not allowed\n");
		exit(0);
	}
	if(target==0)
	{
		printf("Target is null!\n");
	}
	if(target[0]==0)
	{
		printf("Target[0] is null!\n");
	}
	if(target[0]->length > 2)
	{
		printf("-ERROR:SVM from R^n -> R^m not allowed unless m == 1, m is %i atm\n",target[0]->length);
		printf("--Ignoring everyting but target[i]->response[0]\n");
	}

	param.svm_type = NU_SVR;
	param.kernel_type = svm_RBF;
	param.degree = 3;
	param.gamma = 10;//50;
	param.coef0 = 1;//0.01;
	param.nu = 0.5;//0.2;
	param.cache_size = 500;
	param.C = 10;//70;
	param.eps = 1e-3;
	param.p = 1.5;
	param.shrinking = 0;

//	param.nr_weight = 0;
	param.weight_label = NULL;
	param.weight = NULL;

	cross_validation = 1;
	prob.l = nr_data;
	//prob.y = Malloc(double,prob.l);
	prob.y = new double[prob.l];

	prob.x = new svm_node*[nr_data];

	int datadim = data[0]->length;
	x_space = Malloc(struct svm_node,prob.l*(datadim+1));

	float tmp_max = -100000000;
	float * feature_scaling = new float[datadim];
	float * max_f = new float[datadim];
	float * min_f = new float[datadim];
	for(int j = 0; j < datadim; j++){
		max_f[j] = -100000;
		min_f[j] =  100000;
		feature_scaling[j] = 0;
	}

	for(int i = 0;i<prob.l;i++)
	{
		if(target[i]->response[0] > tmp_max)
		{
			tmp_max = target[i]->response[0];
			scaling = target[i]->response[0];
		}
		for(int j = 0; j < datadim; j++){
			if(max_f[j] < data[i]->response[j]){max_f[j] = data[i]->response[j];}
			if(min_f[j] > data[i]->response[j]){min_f[j] = data[i]->response[j];}
			feature_scaling[j]+=data[i]->response[j];
		}
	}
	for(int j = 0; j < datadim; j++){
		feature_scaling[j] /= prob.l;
	}

	int j=0;
	for(int i=0;i<prob.l;i++)
	{
		prob.x[i] = &x_space[j];
		prob.y[i] = (target[i]->response[0])/scaling;
		for(int k = 0; k < datadim;k++)
		{
			x_space[j].index = k;
			x_space[j].value = data[i]->response[k];
			++j;
		}
		x_space[j++].index = -1;
	}

	model = svm_train(&prob,&param);

	float stepsize = 5;
	float small_value = 0.01;
	float shrink_speed = 0.98;
	printf("error pre optimization: %f\n",estimate_error(target, data,nr_data));
	for(int i = 0; i < 50;i++)
	{
		float pre_gamma	= param.gamma;
		float pre_c	= param.C;
		svm_free_and_destroy_model(&model);
		model = svm_train(&prob,&param);
		param.gamma = pre_gamma;
		param.C = pre_c;
		float error_org = estimate_error(target, data,nr_data);
		//printf("error_org %f\n",error_org);


		param.gamma +=small_value;
		svm_free_and_destroy_model(&model);
		model = svm_train(&prob,&param);
		float error_gamma = estimate_error(target, data,nr_data);
		float deriv_gamma = (error_gamma - error_org)/small_value;
		param.gamma = pre_gamma;
		param.C = pre_c;
		//printf("error_gamma %f\n",error_gamma);

		param.C +=1 * small_value;
		svm_free_and_destroy_model(&model);
		model = svm_train(&prob,&param);
		float error_c = estimate_error(target, data,nr_data);
		float deriv_c= (error_c - error_org)/small_value;
		param.gamma = pre_gamma;
		param.C = pre_c;
		//printf("error_c %f\n",error_c);


		//printf("%f = error, gamma: %f C: %f\n",error_org,param.gamma,param.C);
		float length = sqrt(deriv_gamma*deriv_gamma + deriv_c*deriv_c);
		param.gamma 	-= stepsize *deriv_gamma/length;
		param.C 	-= stepsize *deriv_c/length;
		if(param.gamma <= 0){param.gamma = 0.1;}
		if(param.C <= 0){param.C= 0.1;}
		stepsize *= shrink_speed;
	}
	printf("error post optimization: %f\n",estimate_error(target, data,nr_data));

	delete[] feature_scaling;
	delete[] max_f;
	delete[] min_f;
};

float SVMClassifier::estimate_error(FeatureResponse ** target, FeatureResponse ** data, int nr_data)
{
float error = 0;
	for(int i = 0; i < nr_data; i++)
	{
	FeatureResponse * eval = evaluate(data[i]);
	error += fabs(target[i]->response[0] - eval->response[0]);
	delete eval;
	}
return error;
}

using namespace std;
ifstream::pos_type size;
char * memblock;

void SVMClassifier::save(char * path){
	printf("Saving classifier to path:%s\n",path);
	char filename_model[256];
	sprintf(filename_model, "%s.model", path);
	int ret = svm_save_model(filename_model,model);
	if(ret != 0){
		printf("Failed save: ret = %i\n",ret);
	}
	char filename_bin[256];
	sprintf(filename_bin, "%s.bin", path);
	float * data = new float[5];
	data[0] = scaling;
	data[1] = patch->offset_w;
	data[2] = patch->offset_h;
	data[3] = patch->patch_w;
	data[4] = patch->patch_h;
	printf("out: %f %f %f %f %f\n",data[0],data[1],data[2],data[3],data[4]);
	ofstream outfile (filename_bin,ofstream::binary);
	outfile.write ((char * )data,20);
	delete[] data;
	outfile.close();

};



bool SVMClassifier::load(char * path){
	//printf("loading classifier from path:%s\n",path);
	char filename_model[256];
	sprintf(filename_model, "%s.model", path);
	model = svm_load_model(filename_model);
	if(model == 0){
		//printf("load model failed\n");
		return false;
	}
	char filename_bin[256];
	sprintf(filename_bin, "%s.bin", path);
	ifstream file (filename_bin, ios::in|ios::binary|ios::ate);
	if (file.is_open())
	{
		size = file.tellg();
		float * data = new float[5];
		file.seekg (0, ios::beg);
		file.read ((char *)data, size);
		file.close();
		//cout << "the complete file content is in memory\n";
		//printf("in: %f %f %f %f %f\n",data[0],data[1],data[2],data[3],data[4]);
		scaling = data[0];
		patch = new FeaturePatch;
		patch->offset_w = data[1];
		patch->offset_h = data[2];
		patch->patch_w = data[3];
		patch->patch_h = data[4];
		delete[] data;
		return true;
	}
	//else cout << "Unable to open data file\n";
	return false;
};

int SVMClassifier::getCost(){
	if(model == 0){
		return -1;
	}else
	{
		return model->l;
	}
};

};
